/*
 * Copyright (c) 2024, sakumisu
 *
 * SPDX-License-Identifier: Apache-2.0
 */
#include "klibc/kerrno.h"
#include "klibc/kstring.h"
 #include "offline.h"
 #include "rtdef.h"
 #include "rtthread.h"
 #include "rttypes.h"
 #include "usbd_core.h"
 #include "usbd_cdc_acm.h"
 #include <stdint.h>
 #include "vcom.h"
 #include "robotdef.h"
 
 
 #define LOG_TAG "usb"
 #define LOG_LVL LOG_LVL_DBG
 #include <ulog.h>
 
 
 struct Vision_Recv_s Vision_Recv;
 struct Vision_Send_s Vision_Send;
 struct Nav_Recv_s Nav_Recv;
 struct Nav_Send_s Nav_Send;
 
 static rt_uint8_t tmpsend[sizeof(struct Vision_Send_s)]={0};
 static rt_sem_t ep_tx_done_sem =RT_NULL;
 
 #define RING_BUFFER_LEN        CDC_MAX_MPS*4
 static struct rt_ringbuffer *vcom_rb;
 
 
 /*!< endpoint address */
 #define CDC_IN_EP  0x81
 #define CDC_OUT_EP 0x02
 #define CDC_INT_EP 0x83
 
 #if defined SENTRY_BOARD_SMALL_YAW_1 || defined SENTRY_BOARD_BIG_YAW
 
 #define USBD_VID           0xFFFF
 #define USBD_PID           0xFFFF

 #else
 #define USBD_VID           0xFFFF
 #define USBD_PID           0xFFFE
 #endif

 #define USBD_MAX_POWER     100
 #define USBD_LANGID_STRING 1033
 
 /*!< config descriptor size */
 #define USB_CONFIG_SIZE (9 + CDC_ACM_DESCRIPTOR_LEN)
 
 #ifdef CONFIG_USB_HS
 #define CDC_MAX_MPS 512
 #else
 #define CDC_MAX_MPS 64
 #endif
 
 /*!< global descriptor */
 static const uint8_t cdc_descriptor[] = {
     USB_DEVICE_DESCRIPTOR_INIT(USB_2_0, 0xEF, 0x02, 0x01, USBD_VID, USBD_PID, 0x0100, 0x01),
     USB_CONFIG_DESCRIPTOR_INIT(USB_CONFIG_SIZE, 0x02, 0x01, USB_CONFIG_BUS_POWERED, USBD_MAX_POWER),
     CDC_ACM_DESCRIPTOR_INIT(0x00, CDC_INT_EP, CDC_OUT_EP, CDC_IN_EP, CDC_MAX_MPS, 0x02),
     ///////////////////////////////////////
     /// string0 descriptor
     ///////////////////////////////////////
     USB_LANGID_INIT(USBD_LANGID_STRING),
     ///////////////////////////////////////
     /// string1 descriptor
     ///////////////////////////////////////
     0x14,                       /* bLength */
     USB_DESCRIPTOR_TYPE_STRING, /* bDescriptorType */
     'C', 0x00,                  /* wcChar0 */
     'h', 0x00,                  /* wcChar1 */
     'e', 0x00,                  /* wcChar2 */
     'r', 0x00,                  /* wcChar3 */
     'r', 0x00,                  /* wcChar4 */
     'y', 0x00,                  /* wcChar5 */
     'U', 0x00,                  /* wcChar6 */
     'S', 0x00,                  /* wcChar7 */
     'B', 0x00,                  /* wcChar8 */
     ///////////////////////////////////////
     /// string2 descriptor
     ///////////////////////////////////////
     0x26,                       /* bLength */
     USB_DESCRIPTOR_TYPE_STRING, /* bDescriptorType */
     'C', 0x00,                  /* wcChar0 */
     'h', 0x00,                  /* wcChar1 */
     'e', 0x00,                  /* wcChar2 */
     'r', 0x00,                  /* wcChar3 */
     'r', 0x00,                  /* wcChar4 */
     'y', 0x00,                  /* wcChar5 */
     'U', 0x00,                  /* wcChar6 */
     'S', 0x00,                  /* wcChar7 */
     'B', 0x00,                  /* wcChar8 */
     ' ', 0x00,                  /* wcChar9 */
     'C', 0x00,                  /* wcChar10 */
     'D', 0x00,                  /* wcChar11 */
     'C', 0x00,                  /* wcChar12 */
     ' ', 0x00,                  /* wcChar13 */
     'D', 0x00,                  /* wcChar14 */
     'E', 0x00,                  /* wcChar15 */
     'M', 0x00,                  /* wcChar16 */
     'O', 0x00,                  /* wcChar17 */
     ///////////////////////////////////////
     /// string3 descriptor
     ///////////////////////////////////////
     0x16,                       /* bLength */
     USB_DESCRIPTOR_TYPE_STRING, /* bDescriptorType */
     '2', 0x00,                  /* wcChar0 */
     '0', 0x00,                  /* wcChar1 */
     '2', 0x00,                  /* wcChar2 */
     '2', 0x00,                  /* wcChar3 */
     '1', 0x00,                  /* wcChar4 */
     '2', 0x00,                  /* wcChar5 */
     '3', 0x00,                  /* wcChar6 */
     '4', 0x00,                  /* wcChar7 */
     '5', 0x00,                  /* wcChar8 */
     '6', 0x00,                  /* wcChar9 */
 #ifdef CONFIG_USB_HS
     ///////////////////////////////////////
     /// device qualifier descriptor
     ///////////////////////////////////////
     0x0a,
     USB_DESCRIPTOR_TYPE_DEVICE_QUALIFIER,
     0x00,
     0x02,
     0x02,
     0x02,
     0x01,
     0x40,
     0x01,
     0x00,
 #endif
     0x00
 };
 
 USB_NOCACHE_RAM_SECTION USB_MEM_ALIGNX uint8_t read_buffer[CDC_MAX_MPS]; /* 2048 is only for test speed , please use CDC_MAX_MPS for common*/
 USB_NOCACHE_RAM_SECTION USB_MEM_ALIGNX uint8_t write_buffer[CDC_MAX_MPS];
 
 extern void cdc_acm_init(uint8_t busid, uintptr_t reg_base);
 static void usbd_event_handler(uint8_t busid, uint8_t event)
 {
     switch (event) {
         case USBD_EVENT_DISCONNECTED:
             break;
         case USBD_EVENT_RESUME:
             break;
         case USBD_EVENT_SUSPEND:
             break;
         case USBD_EVENT_RESET:
             break;
         case USBD_EVENT_CONNECTED:
             break;
         case USBD_EVENT_CONFIGURED:
             if (ep_tx_done_sem == RT_NULL) {ep_tx_done_sem = rt_sem_create("ep_tx_sem", 1, RT_IPC_FLAG_PRIO);}
             else {rt_sem_release(ep_tx_done_sem);}
             /* setup first out ep read transfer */
             usbd_ep_start_read(busid, CDC_OUT_EP, read_buffer, CDC_MAX_MPS);
             break;
         case USBD_EVENT_SET_REMOTE_WAKEUP:
             break;
         case USBD_EVENT_CLR_REMOTE_WAKEUP:
             break;
 
         default:
             break;
     }
 }
 
 void usbd_cdc_acm_bulk_out(uint8_t busid, uint8_t ep, uint32_t nbytes)
 {
     //USB_LOG_RAW("actual out len:%d\r\n", nbytes);
     // for (int i = 0; i < 100; i++) {
     //     printf("%02x ", read_buffer[i]);
     // }
     // printf("\r\n");
     /* setup next out ep read transfer */
     usbd_ep_start_read(busid, CDC_OUT_EP, read_buffer, CDC_MAX_MPS);
 }
 
 void usbd_cdc_acm_bulk_in(uint8_t busid, uint8_t ep, uint32_t nbytes)
 {
     //USB_LOG_RAW("actual in len:%d\r\n", nbytes);
 
     if ((nbytes % usbd_get_ep_mps(busid, ep)) == 0 && nbytes) {
         /* send zlp */
         usbd_ep_start_write(busid, CDC_IN_EP, NULL, 0);
     }
     if (ep_tx_done_sem == RT_NULL) {ep_tx_done_sem = rt_sem_create("ep_tx_sem", 1, RT_IPC_FLAG_PRIO);}
     else {rt_sem_release(ep_tx_done_sem);}
 }
 
 /*!< endpoint call back */
 struct usbd_endpoint cdc_out_ep = {
     .ep_addr = CDC_OUT_EP,
     .ep_cb = usbd_cdc_acm_bulk_out
 };
 
 struct usbd_endpoint cdc_in_ep = {
     .ep_addr = CDC_IN_EP,
     .ep_cb = usbd_cdc_acm_bulk_in
 };
 
 static struct usbd_interface intf0; 
 static struct usbd_interface intf1;
 
 void cdc_acm_init(uint8_t busid, uintptr_t reg_base)
 {
     usbd_desc_register(busid, cdc_descriptor);
     usbd_add_interface(busid, usbd_cdc_acm_init_intf(busid, &intf0));
     usbd_add_interface(busid, usbd_cdc_acm_init_intf(busid, &intf1));
     usbd_add_endpoint(busid, &cdc_out_ep);
     usbd_add_endpoint(busid, &cdc_in_ep);
     usbd_initialize(busid, reg_base, usbd_event_handler);
 }
 
 
 
 rt_inline void cdc_acm_send(uint8_t busid,rt_uint8_t* buffer,rt_uint8_t size){
     if (ep_tx_done_sem != RT_NULL)
     {
         if (rt_sem_take(ep_tx_done_sem,RT_WAITING_NO)==RT_EOK) {usbd_ep_start_write(busid, CDC_IN_EP, buffer, size);}
         else {return;}
     }
 }
 
 
 void vision_send_process(const struct Vision_Send_s *vision_send)
 {
     // 复制 header
     tmpsend[0] = vision_send->header;
 
     // 复制 bitfield 成员
     tmpsend[1] = vision_send->mode;
 
     // 复制浮点数成员
     rt_memcpy(&tmpsend[2], &vision_send->roll, sizeof(vision_send->roll)); 
     rt_memcpy(&tmpsend[6], &vision_send->pitch, sizeof(vision_send->pitch)); 
     rt_memcpy(&tmpsend[10], &vision_send->yaw, sizeof(vision_send->yaw)); 
 
     tmpsend[sizeof(struct Vision_Send_s)-1] =  vision_send->end;
 
     cdc_acm_send(0,tmpsend, sizeof(struct Vision_Send_s));
 }

 void nav_send_process(const struct Nav_Send_s *nav_send)
 {
    rt_uint8_t tmpdata[sizeof(struct Nav_Send_s)]={0};
    tmpdata[0] = nav_send->header;

    rt_memcpy(&tmpdata[1], &nav_send->current_hp_percent, sizeof(nav_send->current_hp_percent));
    tmpdata[3] = nav_send->game_progress;
    tmpdata[4] = nav_send->event_supply_area_rmuc;
    tmpdata[5] = nav_send->event_supply_area_rmul;
    tmpdata[6] = nav_send->event_center_aera_rmul;

    tmpdata[sizeof(struct Nav_Send_s)-1] =  nav_send->end;
    cdc_acm_send(0,tmpdata, sizeof(struct Nav_Send_s));
 }
  
 
 void vision_recv_process(struct Vision_Recv_s *Vision_Recv)
 {
     if (read_buffer[0]==0X5A && read_buffer[sizeof(struct Vision_Recv_s)-1]==0X5d)
     {   
        rt_memcpy(Vision_Recv, read_buffer, sizeof(struct Vision_Recv_s));
        offline_event_time_update(OFFLINE_MINIPC_HEART);
     }
     rt_memset(read_buffer, 0, sizeof(read_buffer));
 }


void nav_recv_process(struct Nav_Recv_s *Nav_Recv)
{
    if (read_buffer[0]==0X5A && read_buffer[sizeof(struct Nav_Recv_s)-1]==0X5d)
    {   
       rt_memcpy(Nav_Recv, read_buffer, sizeof(struct Nav_Recv_s));
       offline_event_time_update(OFFLINE_MINIPC_HEART);
    }
    rt_memset(read_buffer, 0, sizeof(read_buffer));
}
 
 static void vision_task_thread_entry(void *parameter)
 {
     offline_manage_obj offline_minipc;
     offline_minipc.event = OFFLINE_MINIPC_HEART;
     offline_minipc.offline_time = 1000;
     offline_minipc.error_level = OFFLINE_WARNING_LEVEL;
     offline_minipc.beep_times = 7;
     offline_minipc.enable = OFFLINE_DISABLE;
     offline_minipc.last_time = rt_tick_get();
     offline_minipc.online_state =STATE_OFFLINE;
     offline_event_init(offline_minipc);
     while (1)
     {
        #if defined SENTRY_BOARD_BIG_YAW
            nav_recv_process(&Nav_Recv);
        #else
            vision_recv_process(&Vision_Recv);
        #endif
        rt_thread_mdelay(2);
     }
 }
 
 int usb_sem_init(void){
     /* 创建 线程 */
     rt_thread_t vcomthread = rt_thread_create("vision_task", vision_task_thread_entry, RT_NULL, 1024, 27, 10);
     /* 创建成功则启动线程 */
     if (vcomthread != RT_NULL)
     {
         rt_thread_startup(vcomthread);
     }
     else
     {
         return RT_ERROR;
     }
 
     return RT_EOK;
     LOG_D("usb_init finish\n");
 }
#if defined (SENTRY_BOARD_BIG_YAW) || defined (SENTRY_BOARD_SMALL_YAW_1) || defined (SENTRY_BOARD_SMALL_YAW_2) 
 INIT_APP_EXPORT(usb_sem_init);
#endif
 